// ----------------
// Project:
// ESA Elevator
// ----------------
//
// Description:
// ----------------
// motor_if.v testbench
//
// Version History:
// ----------------
// 140116: added µ to corresponding comment-lines

`timescale 1ns / 1ns

module motor_if_tb_public;
    
  //****************** SIMULATION PARAMETERS *******************//  
  localparam CLK_PERIOD        =    100; //[ns] -> 10 MHz
  localparam MIN_MOTOR_FREQ    =    100; // minimum stepper speed [Hz]
  localparam MAX_MOTOR_FREQ    =     20; // maximum stepper speed [kHz]
  localparam MOTOR_STEP        =     50; // 50 µm
  localparam CONTROL_STEP      =     10; // 10 mm
  localparam MAX_POSITION_R    =   1200; // 1200 control_steps = max. position
  localparam MAX_POSITION_L    =      0; //    0 control_steps = min. position
  localparam INITIAL_POSITION  =      0; // 0 is the initial position
  //***********************************************************// 
  
  //*************** DERIVED SIMULATION PARAMETERS *************// 
  localparam C_0                  = (1000000000 / (CLK_PERIOD * MIN_MOTOR_FREQ));
  localparam C_MIN                = (1000000000 / (CLK_PERIOD * MAX_MOTOR_FREQ * 1000));
  localparam DELAY_COUNT_BITS     = min_1(ceil_log2(C_0));                                                                                                           // 17 bits required to count up to C_0 = 100.000
  localparam CS_TO_MS_CONVERSION  = (CONTROL_STEP   * 1000) / MOTOR_STEP;                                                                                            // to convert control step to motor steps
  localparam DISTANCE_BITS        = min_1(ceil_log2((MAX_POSITION_R * CONTROL_STEP) - (MAX_POSITION_L * CONTROL_STEP)));                                             // 14 bits: max distance 12m = 12.000 mm
  localparam CONTROL_STEP_BITS    = min_1(ceil_log2(MAX_POSITION_R - MAX_POSITION_L));                                                                               // 12m <> 1.200 control steps
  localparam MOTOR_STEP_BITS      = min_1(ceil_log2((MAX_POSITION_R * CONTROL_STEP * CS_TO_MS_CONVERSION) - (MAX_POSITION_L * CONTROL_STEP * CS_TO_MS_CONVERSION))); // 18 bits for 240.000 steps of 50 µm
  localparam MAX_POSITION_R_MM    = MAX_POSITION_R * CONTROL_STEP;
  localparam MAX_POSITION_L_MM    = MAX_POSITION_L * CONTROL_STEP;
  
  //***********************************************************//  
            
                    
    
  //********************* MODULE INPUTS ***********************//  
  reg                     CLK;
  reg                     RESET;
  reg [DISTANCE_BITS-1:0] DISTANCE;
  reg                     ROTATE_RIGHT;
  reg                     ROTATE_LEFT;
  //***********************************************************//  


  //********************* MODULE OUTPUTS **********************//  
  wire       MOTOR_DONE;
  wire       COIL_A;
  wire       COIL_B;
  wire       COIL_C;
  wire       COIL_D;
  //***********************************************************// 


  //******************* UUT INSTANTIATION *********************// 
  motor_if #(.INITIAL_POSITION  (INITIAL_POSITION),
             .MOTOR_STEP        (MOTOR_STEP),
             .CONTROL_STEP      (CONTROL_STEP),
             .DISTANCE_BITS     (DISTANCE_BITS),
             .CONTROL_STEP_BITS (CONTROL_STEP_BITS),
             .MOTOR_STEP_BITS   (MOTOR_STEP_BITS),
             .MAX_POSITION_R    (MAX_POSITION_R), 
             .MAX_POSITION_L    (MAX_POSITION_L), 
             .DELAY_COUNT_BITS  (DELAY_COUNT_BITS),
             .C_0               (C_0), 
             .C_MIN             (C_MIN))
    uut (
        .CLK(CLK),
        .RESET(RESET),
        .DISTANCE(DISTANCE),               
        .ROTATE_RIGHT(ROTATE_RIGHT),
        .ROTATE_LEFT(ROTATE_LEFT),
        .DONE(MOTOR_DONE),
        .CTRL_STEP_DONE(CTRL_STEP_DONE),
        .A(COIL_A),
        .B(COIL_B),
        .C(COIL_C),
        .D(COIL_D));  
  //***********************************************************// 
 

 
  //******************* TESTBENCH SIGNALS *********************//  
  reg [1                  :0] motor_orientation;
  reg [DELAY_COUNT_BITS-1 :0] step_delay_counter;
  reg [CONTROL_STEP_BITS-1:0] control_step_counter;
  reg [1                  :0] motor_orientation_d;          // previous motor orientation
  reg [MOTOR_STEP_BITS-1  :0] motor_step_position;          // motor step position
  reg [MOTOR_STEP_BITS-1  :0] previous_motor_step_position; // previous motor step position
  reg [DELAY_COUNT_BITS-1 :0] current_speed_count;          // speed counter
  reg                         direction; 
  
  time       last_time;
  //***********************************************************//                         


  //****************** TESTBENCH PARAMETERS *******************//   
  localparam MAX_POSITION_MOTOR_STEPS = (MAX_POSITION_R * CONTROL_STEP * 1000) / MOTOR_STEP;
  localparam MIN_POSITION_MOTOR_STEPS = (MAX_POSITION_L * CONTROL_STEP * 1000) / MOTOR_STEP; 
  
  localparam MM_TO_MS_CONVERSION  = 1000 / MOTOR_STEP;                    // to convert mm to motor steps
  //***********************************************************//
  
 
  //****************** CONTROL STEP COUNTER *******************//  
  always@(posedge CLK) begin
    if (RESET) begin
      control_step_counter     <= {(CONTROL_STEP_BITS){1'b0}};
    end
    else begin
      if (ROTATE_RIGHT | ROTATE_LEFT) begin
        control_step_counter     <= {(CONTROL_STEP_BITS){1'b0}};      
      end
      else if (CTRL_STEP_DONE) begin
        control_step_counter     <= control_step_counter + 1'b1;
      end
    end
  end
  //***********************************************************//


  //********************* COIL CONVERSION *********************//   
  always @(*) begin
    case ({COIL_A, COIL_B, COIL_C, COIL_D})
      4'b1001: motor_orientation = 2'b00;
      4'b1100: motor_orientation = 2'b01;
      4'b0110: motor_orientation = 2'b10;
      4'b0011: motor_orientation = 2'b11;
      default: motor_orientation = 2'b00;
    endcase    
  end
  //***********************************************************//
  

  //******************* TESTBENCH POSITION ********************//  
  always@(posedge CLK) begin
    if (RESET) begin
      step_delay_counter  <= {(DELAY_COUNT_BITS){1'b0}};
      motor_step_position <= {(MOTOR_STEP_BITS){1'b0}};
      motor_orientation_d <= motor_orientation;
      current_speed_count <= {(DELAY_COUNT_BITS){1'b0}};
      direction           <= 1'b1;
    end
    else begin
      motor_orientation_d <= motor_orientation;    
      if (motor_orientation == (motor_orientation_d + 1'b1)) begin
        step_delay_counter  <= {(DELAY_COUNT_BITS){1'b0}};
        current_speed_count <= step_delay_counter;
        motor_step_position <= motor_step_position + 1'b1;
        direction           <= 1'b1;
      end
      else if (motor_orientation == (motor_orientation_d - 1'b1)) begin
        step_delay_counter  <= {(DELAY_COUNT_BITS){1'b0}};
        current_speed_count <= step_delay_counter;
        motor_step_position <= motor_step_position - 1'b1;
        direction           <= 1'b0;
      end
      else begin      
        step_delay_counter  <= step_delay_counter + 1'b1;  
      end
    end
  end
  //***********************************************************//

   
  //******************* 10 MHz CLOCK SIGNAL *******************//   
  always begin 
    #(CLK_PERIOD/2) CLK = ~CLK;
  end
  //***********************************************************// 
  
  //********************* TEST INITIATION *********************//  
  initial begin
    // Initialize Inputs
    CLK                = 0;
    RESET              = 1;
    ROTATE_LEFT        = 0;
    ROTATE_RIGHT       = 0;
    DISTANCE           = 'd000;

    // Wait 100 ns for global reset to finish
    #(3*CLK_PERIOD/2);
    RESET = 0;
    
    $display("************ STARTING SIMULATION ************");  
    //--------------------------
    // 0.1 ROTATE RIGHT 1000 => 1m
    //--------------------------
    $display("motor right rotate 1000 mm");
    previous_motor_step_position = 0;
    last_time          = $time;
    ROTATE_RIGHT       = 1;
    DISTANCE           = 'd1000;
    #(CLK_PERIOD);
    ROTATE_RIGHT       = 0;
    #(CLK_PERIOD);
    while (~MOTOR_DONE && (motor_step_position != (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end 
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      if(motor_step_position == (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION)) begin
        $display("%d ms: motor stopped correctly @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
        $display("received %d control steps of %d  mm", control_step_counter, CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("%d ms: motor did not stop correctly @ position %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);   
    end 
    #(5*CLK_PERIOD);
    //--------------------------
    // 0.2 ROTATE RIGHT 2000 => 3m
    //--------------------------
    $display("motor right rotate 2000 mm");
    previous_motor_step_position      = motor_step_position;
    last_time          = $time;
    ROTATE_RIGHT       = 1;
    DISTANCE           = 'd2000;
    #(CLK_PERIOD);
    ROTATE_RIGHT       = 0;
    #(CLK_PERIOD);
    while (~MOTOR_DONE && (motor_step_position != (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end 
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      if(motor_step_position == (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION)) begin
        $display("%d ms: motor stopped correctly @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
        $display("received %d control steps of %d  mm", control_step_counter, CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("%d ms: motor did not stop correctly @ position %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      //while (~MOTOR_DONE) begin
      //#(CLK_PERIOD);
      //end
      //$display("%d ms: motor done received @ motor_step_position %d / %d mm", $time/1000000, motor_step_position, motor_step_position/MM_TO_MS_CONVERSION);
    end 
    #(5*CLK_PERIOD);
    //--------------------------
    // 0.3 ROTATE LEFT 3000 => 0m
    //--------------------------
    $display("motor left rotate 3000 mm");
    previous_motor_step_position      = motor_step_position;
    last_time          = $time;
    ROTATE_LEFT        = 1;
    DISTANCE           = 'd3000;
    #(CLK_PERIOD);
    ROTATE_LEFT        = 0;
    #(CLK_PERIOD);
    while (~MOTOR_DONE && (motor_step_position != (previous_motor_step_position - DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end 
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      if(motor_step_position == (previous_motor_step_position - DISTANCE*MM_TO_MS_CONVERSION)) begin
        $display("%d ms: motor stopped correctly @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
        $display("received %d control steps of %d  mm", control_step_counter, CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("%d ms: motor did not stop correctly @ position %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);   
    end 
    #(5*CLK_PERIOD);         
    //--------------------------
    // 1. ROTATE RIGHT 6000 => 6m (2nd floor)
    //--------------------------
    $display("motor right rotate 6000 mm");
    previous_motor_step_position      = 0;
    last_time          = $time;
    ROTATE_RIGHT       = 1;
    DISTANCE           = 'd6000;
    #(CLK_PERIOD);
    ROTATE_RIGHT       = 0;
    #(CLK_PERIOD);
    while (~MOTOR_DONE && (motor_step_position != (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end 
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      if(motor_step_position == (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION)) begin
        $display("%d ms: motor stopped correctly @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
        $display("received %d control steps of %d  mm", control_step_counter, CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("%d ms: motor did not stop correctly @ position %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);   
    end 
    #(5*CLK_PERIOD);
    //--------------------------
    // 2. ROTATE RIGHT 3000 => 9m (3rd floor)
    //--------------------------
    $display("motor right rotate 3000 mm");
    previous_motor_step_position      = motor_step_position;
    last_time          = $time;
    ROTATE_RIGHT       = 1;
    DISTANCE           = 'd3000;
    #(CLK_PERIOD);
    ROTATE_RIGHT       = 0;
    #(CLK_PERIOD);
    while(~MOTOR_DONE && (motor_step_position != (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end 
    if (MOTOR_DONE) begin
        #(CLK_PERIOD);
      if(motor_step_position == (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION)) begin
        $display("%d ms: motor stopped correctly @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
        $display("received %d control steps of %d  mm", control_step_counter, CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("%d ms: motor did not stop correctly @ position %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION); 
    end
    #(5*CLK_PERIOD);
    //--------------------------
    // 3. ROTATE RIGHT 6000 (only 3000 possible) => 12m (4th floor) 
    //--------------------------
    $display("motor right rotate 6000 mm");
    previous_motor_step_position      = motor_step_position;
    last_time          = $time;
    ROTATE_RIGHT       = 1;
    DISTANCE           = 'd6000;
    #(CLK_PERIOD);
    ROTATE_RIGHT       = 0;
    #(CLK_PERIOD);
    while(~MOTOR_DONE && (motor_step_position != (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      if(motor_step_position == (MAX_POSITION_MOTOR_STEPS)) begin
        $display("%d ms: motor stopped correctly @ max. position of %d mm", $time/1000000, MAX_POSITION_R);
        $display("last run was only %d mm, max. position reached", control_step_counter*CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("%d ms: motor did not stop correctly @ max. position of %d mm", $time/1000000, MAX_POSITION_R_MM);
      $display("aborting simulation...");
      $finish;
    end 
    #(5*CLK_PERIOD);
    //--------------------------
    // 4. ROTATE LEFT 9000 => 3m (1st floor)
    //--------------------------
    $display("motor left rotate 9000 mm");
    previous_motor_step_position      = motor_step_position;
    last_time          = $time;
    ROTATE_LEFT        = 1;
    DISTANCE           = 'd9000;
    #(CLK_PERIOD);
    ROTATE_LEFT        = 0;
    #(CLK_PERIOD);
    while(~MOTOR_DONE && (motor_step_position != (previous_motor_step_position - DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      if(motor_step_position == (previous_motor_step_position - DISTANCE*MM_TO_MS_CONVERSION)) begin
        $display("%d ms: motor stopped correctly @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
        $display("received %d control steps of %d  mm", control_step_counter, CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("%d ms: motor did not stop correctly @ position %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION); 
    end
    #(5*CLK_PERIOD);
    //--------------------------
    // 5. ROTATE LEFT 6000 => 0m (ground floor) (only 300 possible)
    //--------------------------
    $display("motor left rotate 6000 mm");
    previous_motor_step_position      = motor_step_position;
    last_time          = $time;
    ROTATE_LEFT        = 1;
    DISTANCE           = 'd6000;
    #(CLK_PERIOD);
    ROTATE_LEFT        = 0;
    #(CLK_PERIOD);  
    while(~MOTOR_DONE && (motor_step_position != (previous_motor_step_position - DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      if(motor_step_position == (MAX_POSITION_L)) begin
        $display("%d ms: motor stopped correctly @ %d cm", $time/1000000, MAX_POSITION_L);
        $display("last run was only %d mm, min. position reached", control_step_counter*CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("motor did not stop correctly @ min. position of %d mm", MAX_POSITION_L_MM);
      $display("aborting simulation...");
      $finish;
    end 
    #(5*CLK_PERIOD);
    //--------------------------
    // 6. ROTATE RIGHT 9000 / UPDATE ROTATE 3000 @ 1st FLOOR
    //--------------------------
    $display("motor right rotate 9000 mm");
    previous_motor_step_position      = motor_step_position;
    last_time          = $time;
    ROTATE_RIGHT       = 1;
    DISTANCE           = 'd9000;
    #(CLK_PERIOD);
    ROTATE_RIGHT       = 0;
    #(CLK_PERIOD);
    while(~MOTOR_DONE && (motor_step_position != (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION/3))) begin
      #(CLK_PERIOD);
    end
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
    end
    else begin
      #(CLK_PERIOD);
      $display("updating motor target: right rotate 3000 mm from current position %d mm", motor_step_position/MM_TO_MS_CONVERSION);
      previous_motor_step_position      = motor_step_position;
      last_time          = $time;
      ROTATE_RIGHT       = 1;
      DISTANCE           = 'd3000;
      #(CLK_PERIOD);
      ROTATE_RIGHT       = 0;
      #(CLK_PERIOD);
    end
    while(~MOTOR_DONE && (motor_step_position != (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION))) begin
      #(CLK_PERIOD);
    end
    if (MOTOR_DONE) begin
      #(CLK_PERIOD);
      if(motor_step_position == (previous_motor_step_position + DISTANCE*MM_TO_MS_CONVERSION)) begin
        $display("%d ms: motor stopped correctly @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
        $display("received %d control steps of %d  mm", control_step_counter, CONTROL_STEP);
        $display("this took %d ms", ($time-last_time)/1000000);
      end
      else begin
        $display("%d ms: unexpected motor stop @ %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION);
      end
    end
    else begin
      #(CLK_PERIOD);
      $display("%d ms: motor did not stop correctly @ position %d mm", $time/1000000, motor_step_position/MM_TO_MS_CONVERSION); 
    end     
    #(5*CLK_PERIOD);
    $display("************ SIMULATION COMPLETE ************");
    $finish;
  end
  //***********************************************************//initial begin : kill
//end

initial begin : kill
  integer i;
  for (i = 0; i < 10000; i=i+1)
  begin
	#(1000000*CLK_PERIOD);
  end
  $display("************ SIMULATION KILLED BECAUSE OF ERROR ************");
  $finish;
end

 //****************** VISUALISATION PROCESS ******************//   
  always@(direction) begin
    if (RESET) begin
    end
    else begin
      if (direction)
        $display("changing direction -> going up");
      else
        $display("changing direction -> going down");
    end
  end
  //***********************************************************//


  //******************* PARAMETER FUNCTIONS *******************//
  //ceil of the log base 2
  function integer ceil_log2;
    input [31:0] value;
    for (ceil_log2=0; value>0; ceil_log2=ceil_log2+1)
      value = value>>1;
  endfunction
  
  // value cannot be less than 1
  function integer min_1;
    input [31:0] value;
    if (value == 0)
      min_1 = 1;
    else
      min_1 = value;
  endfunction 
  //***********************************************************//
    
endmodule

